/* 
 *  Incub - incubate your baby robot
 *  Copyright (C) 2008 Stanislaw Szymczyk
 *  e-mail: sszymczy@gmail.com   www: http://cogville.cog-lab.com
 *
 *  This file is part of Incub.
 *
 *  Incub is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  Incub is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with Incub.  If not, see <http://www.gnu.org/licenses/>.
 * 
 */
 
#include <iostream>
#include <cmath>
#include <exception>
#include <string>

#include "incub.hh"
#include "ClientCallback.h"
#include "CorbaImplementations.h"
#include "CorbaHelper.h"
#include "boost/program_options.hpp"
#include <boost/thread/mutex.hpp>
#include "boost/format.hpp"
#include <GL/glut.h>

using boost::format;
namespace po = boost::program_options;
ClientCallback *callback;
using std::string;

VWorldIn_var worldIn;

class CarCallback : public ClientCallback {
	private:
	IGate_var gate;
	string path;
	int counter;
	public:
	CarCallback(string path, IGate_var gate) {
		counter = 0;
		this->path = path;
		this->gate = gate;
	}
	void call() {
		std::cout << "callback called!" << std::endl;

		VWorldOut_var world = gate->getVWorldOut();
		if(counter % 200 == 0) {
			for(int i = 0; i < 4; i++) {
				VMotorIn& motor = *(*(*(*worldIn->entities())[3]->joints())[i]->motors())[0];
				VVelocityControlledMotorIn::_downcast(&motor)->maxForce(5.0);
				VVelocityControlledMotorIn::_downcast(&motor)->desiredVelocity(10.0);
			}
		}
		if((counter + 100) % 200 == 0) {
			for(int i = 0; i < 4; i++) {
				VMotorIn& motor = *(*(*(*worldIn->entities())[3]->joints())[i]->motors())[0];
				VVelocityControlledMotorIn::_downcast(&motor)->maxForce(5.0);
				VVelocityControlledMotorIn::_downcast(&motor)->desiredVelocity(-10.0);
			}
		}
		gate->setVWorldIn(worldIn);
		counter++;
	}
};

class ExitException : public std::exception {
};

int main(int argc, char** argv)
{
	CORBA::ORB_var orb;
	CORBA::BOA_var boa;
	IGate_var gate;

	try {
		orb = CORBA::ORB_init(argc, argv, "omniORB4");

		CorbaHelper::registerFactories(orb);

		boa = orb->BOA_init(argc, argv, "omniORB4_BOA");
	
		CORBA::Object_var obj = CorbaHelper::getGateReference(orb);
	
		gate = IGate::_narrow(obj);

		
		obj = orb->resolve_initial_references("RootPOA");
		PortableServer::POA_var poa = PortableServer::POA::_narrow(obj);

		PortableServer::POAManager_var pman = poa->the_POAManager();
		pman->activate();
	
		worldIn = gate->getVWorldIn();
	
		CarCallback *callback = new CarCallback(string(""), gate);
		PortableServer::ObjectId_var myCallbackId = poa->activate_object(callback);
		gate->registerCallback(callback->_this());

		for(int i = 0; i < 1; i++) {
			sleep(10);
		}
			
		gate->unregisterCallback(callback->_this());
		poa->deactivate_object(myCallbackId);
		delete callback;

		orb->destroy();

	} catch(CORBA::COMM_FAILURE& ex) {
		std::cerr << "Caught system exception COMM_FAILURE -- unable to contact the "
		 << "object." << std::endl;
		 exit(1);
	} catch(CORBA::MARSHAL& ex) {
		std::cerr << "Caught MARSHAL exception " << ex.NP_minorString() << std::endl;
		 exit(1);
	} catch(CORBA::BAD_PARAM& ex) {
		std::cerr << "Caught BAD_PARAM exception " << ex.NP_minorString() << std::endl;
		 exit(1);		
	} catch(CORBA::SystemException& ex) {
		std::cerr << "Caught a CORBA::SystemException."<< std::endl;
		 exit(1);
	} catch(CORBA::Exception& ex) {
		std::cerr << "Caught CORBA::Exception." << std::endl;
		 exit(1);
	} catch(omniORB::fatalException& fe) {
		std::cerr << "Caught omniORB::fatalException:" << std::endl;
		std::cerr << "file: " << fe.file() << std::endl;
		std::cerr << "line: " << fe.line() << std::endl;
		std::cerr << "mesg: " << fe.errmsg() << std::endl;
		 exit(1);
	} catch(...) {
		std::cerr << "Caught unknown exception." << std::endl;
		 exit(1);
	}
  
	return 0;
}
